#include <iostream>
#include <climits>
#include <vector>
#include <LEDA/numbers/fp.h>
#include <LEDA/graphics/graphwin.h>
#include <LEDA/graphics/color.h>
#include <LEDA/core/set.h>
#include <LEDA/core/tuple.h>
#include <LEDA/core/queue.h>
#include <LEDA/core/p_queue.h>
#include <LEDA/graphics/window.h>
#include <LEDA/graph/graph.h>
#include "control.h"
#include <cmath>

using namespace leda;

#define MIN(x, y) x < y ? x : y

// define infinity as a large number, in a way that if we add values to it, it will not overflow
#define INF 9999999

void color_edge(GraphWin &gw, edge &e, edge_array<two_tuple<int,int> > &capacity_forw_back)
{
	// color edge accordingly, blue - forward edge, red - backward edge, both - orange
	if (capacity_forw_back[e].first() != 0 && capacity_forw_back[e].second() == 0){
    	gw.set_color(e , blue);
	}
	else if (capacity_forw_back[e].first() == 0 && capacity_forw_back[e].second() != 0){
    	gw.set_color(e , red);
	}
	else if (capacity_forw_back[e].first() != 0 && capacity_forw_back[e].second() != 0){
    	gw.set_color(e , orange);
	}
}

void update_forw_back_capacity(edge_array<two_tuple<int,int> > &capacity_forw_back,
		node_array<int> &excess, edge_array<int> &capacity, edge_array<int> &preflow, edge &e)
{
	// update with new forward and backward edge capacities

	// forward edge
	if(preflow[e] < capacity[e]){
		capacity_forw_back[e].first() = capacity[e] - preflow[e];
	}
	else{
		capacity_forw_back[e].first() = 0;
	}

	// backward edge
	if (preflow[e] > 0){
		capacity_forw_back[e].second() = preflow[e];
	}
	else{
		capacity_forw_back[e].second() = 0;
	}
}

void calculate_excess(node &v, node_array<int> &excess, edge_array<int> &preflow)
{
	edge e_e;
	int in_preflow = 0;
	forall_in_edges(e_e, v){
		in_preflow += preflow[e_e];
	}
	int out_preflow = 0;
	forall_out_edges(e_e, v){
		out_preflow += preflow[e_e];
	}
	excess[v] = in_preflow - out_preflow;
}

void bfs(GraphWin &gw, graph &g, node parent, queue<two_tuple<node,int> > &q, node_array<int> &bfsnum,
		node_array<int> &marker, int &akt, node_array<int> &distance, node &source, node &sink)
{
	int depth;
	node v;
    edge e;

	if(q.empty()){
		return;
	}

	//our new current node and depth
	two_tuple<node,int> tuple = q.pop();
	v = tuple.first();
	depth = tuple.second();
    bfsnum[v] = akt++;                              // BFS-Nummer zuweisen

	 // GOLDBERG STUFF (distance function initialization)
    if (v == source){
    	distance[v] = g.number_of_nodes();
        gw.set_label(v, string("S, d:%i", distance[v]));
    }
    else if (v == sink){
    	distance[v] = depth;
        gw.set_label(v, string("T, d:%i", distance[v]));
    }
    else{
    	distance[v] = depth;
        gw.set_label(v, string("e:0, d:%i", distance[v]));
    }
    // -END- GOLDBERG STUFF (distance function initialization)

	gw.set_border_color(v , grey3);
	gw.set_color(v , grey1);
    control_wait(0.5);

    forall_inout_edges(e, v) {  // alle Nachbarkanten von v
        node w = g.opposite(v, e); // Knoten am anderen Ende von e
        if (w != parent) {     // die Kante zum parent ignorieren wir
            if (marker[w] < 0 && bfsnum[w] < 0) { // falls Knoten w noch nicht besucht
            	//appending next reachable adjacent node, and incrementing depth
            	two_tuple<node,int> t (w,(depth + 1));
            	q.append(t);
            	marker[w] = 1; // node visited
            }
        }
    }

    bfs(gw, g, v, q, bfsnum, marker, akt, distance, source, sink); // recursive call for new q
}

void init_distance(graph & g, GraphWin & gw, node & sink, node_array<int> & distance, node & source)
{
    int akt = 0;
    node_array<int> bfsnum(g, -1);
    node_array<int> marker(g, -1);
    gw.message("Running bfs in order to get the distances!!");
    do{
        queue<two_tuple<node,int> > q;
        two_tuple<node,int> t(sink, 0);
        q.append(t);
        bfs(gw, g, sink, q, bfsnum, marker, akt, distance, source, sink);
    } while(akt < g.number_of_nodes());
    gw.del_message();
}

void init_preflow(graph &g, GraphWin &gw, edge_array<int> &capacity, edge_array<int> &preflow,
		edge_array< two_tuple<int,int> > &capacity_forw_back, node &source, node &sink,
		queue<node> &active_nodes, node_array<bool> &active_marker)
{
    edge e;
    forall_edges(e, g)
    {
    	// color current edge
    	gw.set_color(e , green);
    	control_wait(0.5);

    	node t = g.target(e);
    	node s = g.source(e);

    	// set edge capacity values
    	capacity[e] = atoi(gw.get_label(e));

    	// now all the edges are forward, we set their capacities in the residual network also
    	capacity_forw_back[e].first() = atoi(gw.get_label(e));

    	// if s is source initialize preflow
		if (s == source){
			// preflow takes the value of the edges capacity
			preflow[e] = capacity[e];

			// how the preflow equals the capacity, the forward edge dissapears, its capacity will be 0
	    	capacity_forw_back[e].first() = 0;
	    	// now the backward edge appears with capacity preflow[e]
	    	capacity_forw_back[e].second() = preflow[e];

	    	if (t != sink){
	    		// if t differs of sink add it to the active nodes queue and mark it as an active node
	    		active_nodes.append(t);
	    		active_marker[t] = true;
	    		// color the active nodes
	    		gw.set_color(t , blue);
		    	control_wait(0.5);
	    	}
		}

    	// set edge label as 'preflow / capacity'
    	gw.set_label(e,string("%i/%i",preflow[e], capacity[e]));
        gw.redraw();
    	control_wait(0.5);
    	// color edge accordingly, blue - forward edge, red - backward edge, both - orange
    	color_edge(gw, e, capacity_forw_back);
    }
}

void push(GraphWin &gw, graph &g, edge_array<int> &preflow, edge_array<int> &capacity, node_array<int> &excess,
		edge_array< two_tuple<int,int> > &capacity_forw_back, node_array<int> &distance, queue<node> &active_nodes,
		node_array<bool> &active_marker, node &source, node &sink, node &v, node &w, edge &e, int type_marker)
{
	int push_value;
	// amount of flow from v to w
	// if forward edge
	if (type_marker == 1){
		push_value = MIN(excess[v], capacity[e] - preflow[e]);
		preflow[e] += push_value;
		//std::cout << "**Forward edge: Push value: " << push_value << std::endl;
	}
	// if backward edge
	if (type_marker == 2){
		push_value = MIN(excess[v], preflow[e]);
		preflow[e] -= push_value;
		//std::cout << "**Backward edge: Push value: " << push_value << std::endl;
	}

	// update with new forward and backward edge capacities
	update_forw_back_capacity(capacity_forw_back, excess, capacity, preflow, e);

	// rewrite edge label with new 'preflow / capacity'
	gw.set_label(e,string("%i/%i",preflow[e], capacity[e]));
	gw.redraw();
	control_wait(0.5);

	// excess value of v decreased with the push value
	excess[v] -= push_value;
	// rewrite node label
	gw.set_label(v, string("e:%i, d:%i", excess[v], distance[v]));
	control_wait(0.5);

	// excess value of w increased with the push value
	excess[w] += push_value;
	// rewrite node label
	if ((w != source) && (w != sink)){
		gw.set_label(w, string("e:%i, d:%i", excess[w], distance[w]));
		control_wait(0.5);
	}

	// is w differs of source or sink, and w was not active before, add it to the active nodes
	if ((w != source) && (w != sink) && (!active_marker[w]))
	{
		active_nodes.append(w);
		active_marker[w] = true;
	}
}

void relabel(GraphWin &gw, graph &g, node_array<int> &distance, node_array<int> &excess,
		queue<node> &active_nodes, node_array<bool> &active_marker,
		edge_array<two_tuple<int,int> > &capacity_forw_back, node &v)
{
	int min_distance = 0;

	edge e;
	// for all OUT edges check only forward
	forall_out_edges(e,v){
		if(capacity_forw_back[e].first() > 0){
			node w = g.target(e);
			// if min_distance not yet instantiated
			if (min_distance == 0){
				min_distance = distance[w];
			}
			min_distance = MIN(min_distance, distance[w]);
		}
	}

	// for all IN edges check only backward
	forall_in_edges(e,v){
		if(capacity_forw_back[e].second() > 0){
			node w = g.source(e);
			// if min_distance not yet instantiated
			if (min_distance == 0){
				min_distance = distance[w];
			}
			min_distance = MIN(min_distance, distance[w]);
		}

	}

	distance[v] = min_distance + 1;

	active_nodes.append(v);
	active_marker[v] = true;

	gw.set_label(v, string("e:%i, d:%i", excess[v], distance[v]));
	control_wait(0.5);
}


void goldberg(GraphWin &gw, graph &g, node &source, node &sink)
{
	// preflow and capacity of G
    edge_array<int> preflow(g, 0);
    edge_array<int> capacity(g, 0);

    node_array<int> excess(g, 0);
    node_array<int> distance(g, 0);

    // marker to set if the node is an active node or not
    node_array<bool> active_marker(g, false);

    queue<node> active_nodes;
    node curr_node;

    // Edge array with two_tuple<int,int> which represent the capacity
    // of the forward(first) and the backward(second) edges of the residual network (G')
    edge_array< two_tuple<int,int> > capacity_forw_back(g, two_tuple<int,int>(0,0));

    // excess of source is infinity
    excess[source] = INF;

    ///////////////////INIT DISTANCE FUNCTION
    // Initializing the distance function
    init_distance(g, gw, sink, distance, source);


    ///////////////////INIT PREFLOW
    // Initializing the preflow
    gw.message("EDGE: green - current; blue - forward; red - backward; orange - both;"
    		"    NODE: green - current; blue - active; red - target");
    init_preflow(g, gw, capacity, preflow, capacity_forw_back,
    		source, sink, active_nodes, active_marker);


    ///////////////////MAIN LOOP
    // Main loop
    while(!active_nodes.empty())
    {
    	// pop the node out of the active nodes queue
    	curr_node = active_nodes.pop();

    	// color the current node
		gw.set_color(curr_node , green);

    	// curr node is not longer in the active nodes, we mark it as false
    	active_marker[curr_node] = false;

    	// calculate the excess of curr_node: SUM all in edges preflow - SUM all out edges preflow
    	calculate_excess(curr_node, excess, preflow);

    	// update label
    	gw.set_label(curr_node, string("e:%i, d:%i", excess[curr_node], distance[curr_node]));
		control_wait(0.5);

		// type marker: 1-forward edge, 2-backward edge
		int type_marker;
		// loop through all edges, and check for forward edges from the curr_node in the residual network (G')
		edge e;
		// first for all out edges in G, check if we have forward residual edges in G'
		forall_out_edges(e, curr_node)
		{
			// color the current edge
			gw.set_color(e , green);
			control_wait(0.5);

			// continue if we have a forward edge
			if (capacity_forw_back[e].first() > 0)
			{
				// set marker to 1 (forward)
				type_marker = 1;
				// w is the target node (edge being forward in G') of the edge
				node w = g.target(e);
				// color the targeted node to red
				gw.set_color(w , red);
				control_wait(0.5);

				// if excess of curr node is bigger than 0 and
				// the distance between the current node and the adj one is 1, then apply a push
				if ((excess[curr_node] > 0) && (distance[curr_node] == distance[w] + 1))
				{
					///////////// PUSH
					push(gw, g, preflow, capacity, excess, capacity_forw_back, distance, active_nodes,
							active_marker, source, sink, curr_node, w, e, type_marker);
				}

				// set the corresponding color to the target node w
				if (active_marker[w]){
					gw.set_color(w , blue);
				}
				else{
					gw.set_color(w , grey1);
				}
				control_wait(0.5);
			}

	    	// color edge accordingly, blue - forward edge, red - backward edge, both - orange
	    	color_edge(gw, e, capacity_forw_back);
		}

		// secondly for all in edges in G, check if we have backward residual edges in G'
		forall_in_edges(e, curr_node)
		{
			// color the current edge
			gw.set_color(e , green);
			control_wait(0.5);

			// continue if we have a backward edge
			if (capacity_forw_back[e].second() > 0)
			{
				// set marker to 2 (backward)
				type_marker = 2;
				// w is the source node (edge being backward in G') of the edge
				node w = g.source(e);
				// color the targeted node to red
				gw.set_color(w , red);
				control_wait(0.5);

				// if excess of curr node is bigger than 0 and
				// the distance between the current node and the adj one is 1, then apply a push
				if ((excess[curr_node] > 0) && (distance[curr_node] == distance[w] + 1))
				{
					///////////// PUSH
					push(gw, g, preflow, capacity, excess, capacity_forw_back, distance, active_nodes,
							active_marker, source, sink, curr_node, w, e, type_marker);
				}

				// set the corresponding color to the target node w
				if (active_marker[w]){
					gw.set_color(w , blue);
				}
				else{
					gw.set_color(w , grey1);
				}
				control_wait(0.5);
			}
	    	// color edge accordingly, blue - forward edge, red - backward edge, both - orange
	    	color_edge(gw, e, capacity_forw_back);
		}

		// RELABEL if excess of curr node > 0
		if ((excess[curr_node] > 0))
		{
			///////////// RELABEL
			relabel(gw, g, distance, excess, active_nodes, active_marker, capacity_forw_back, curr_node);
		}

		// set the corresponding color to the current node
		if (active_marker[curr_node]){
			gw.set_color(curr_node , blue);
		}
		else{
			gw.set_color(curr_node , grey1);
		}
		control_wait(0.5);

    }

    // calculating the max flow
    int max_flow = 0;
    edge e;
    forall_in_edges(e,sink){
    	max_flow += preflow[e];
    }
    gw.message(string("Maximum flow: %i",max_flow));



}

int main(int argc, char *argv[]) {

    GraphWin gw(800, 600);
    gw.display();
    create_control();
    gw.set_directed(false);

    if (argc > 1)
        gw.read(argv[1]);
    gw.edit();
    graph &g = gw.get_graph();
    if (g.number_of_nodes() == 0) {
        gw.close(); destroy_control();
        exit(1);
    }

    // variables
    node source, sink;

    // select the SOURCE node
    while ((source = gw.read_node()) == NULL);

    // set the label of SOURCE
    gw.set_label(source, "S");

    // select the SINK node
    while ((sink = gw.read_node()) == NULL);

    // set the label of SINK
    gw.set_label(sink, "T");

    control_wait(0.5);

    // change node shape to rectangle for visibility
    gw.set_node_shape(rectangle_node);
    gw.set_node_width(80);
    control_wait(0.5);

	/*
	 * Call goldberg & tarjan push relabel algorithm
	 */
    goldberg(gw, g, source, sink);



    gw.acknowledge("Ready!");
    gw.edit();
    gw.close();
    destroy_control();
    exit(0);
}
























